import * as THREE from "../../../modules/three/three.module.js";
import {d2r, r2d} from "../lib/CommonConsts.js";
import {findTriangleNeighbors} from "./ShapeExtract.js";
import {arrAsubB, isBetween} from "../lib/util/ToolMethods.js";

const uZ = new THREE.Vector3(0, 0, 1);
const uY = new THREE.Vector3(0, 1, 0);
const _quat = new THREE.Quaternion().setFromUnitVectors(uZ, uY);
const _normal = new THREE.Vector3();
const _spherical = new THREE.Spherical();
const _plane = new THREE.Plane();
const _v0 = new THREE.Vector3();
const _v1 = new THREE.Vector3();
const _v2 = new THREE.Vector3();
const _v3 = new THREE.Vector3();
function getTriangleAngles(triangle) {
  const {a, b, c} = triangle;
  const ba = _v0.subVectors( a, b ).normalize();
  const bc = _v1.subVectors( c, b ).normalize();
  const ac = _v2.subVectors( c, a ).normalize();
  const ab = _v3.copy(ba).negate();
  const angleB = Math.acos(ba.dot(bc)) * r2d;
  const angleA = Math.acos(ac.dot(ab)) * r2d;
  const angleC = 180 - angleA - angleB;
  return [angleA, angleB, angleC];
}

function getTriangleAnglesMin(triangle) {
  return Math.min(...getTriangleAngles(triangle));
}

// function isBe

export const ConfigRoute = {
  // 方位角容忍度
  azimuthAmplitude: 60 * d2r,
  // 如果某个位置的俯仰角度数在区间内，比如 [0° ~ 45°] 或 [135° ~ 180°] 可以认为喷涂这些区域需要几乎水平的航线
  horizontalRanges: [[0, 30 * d2r], [150 * d2r, 180 * d2r]],
  // 喷涂网格可能存在微小的凹凸区域，这些地方的法向量剧烈的变化
  // 三角形的面积小于这个数时，不考虑其法向量，直接并入当前区域, 单位为平方米
  ignoreArea: 0.005,
  // 参考平面 到参考喷涂顶点的距离
  planeDistance: 1,
  // 第二步，离散化时，使用的格网中的 单个正方形格的边长，单位为米
  cellSize: 0.04,

  isHorizontal(pitch) {
    const [[a, b], [c, d]] = this.horizontalRanges;
    return isBetween(pitch, a, b) || isBetween(pitch, c, d);
  }
}

class TriangleSet {
  isHorizontal = false
  // 均值法向
  meanNormal = new THREE.Vector3()
  center = new THREE.Vector3()
  // 区域面积
  area = 0
  triangles = []
  // 二维轮廓
  contour = []

  constructor(triangles, isHorizon) {
    this.triangles = triangles;
    this.isHorizontal = isHorizon;
    const normal = new THREE.Vector3();
    triangles.forEach(tri => {
      const a = tri.getArea();
      this.area += a;
      tri.getNormal(_normal).multiplyScalar(a);
      normal.add(_normal);
      this.center.add(tri.a).add(tri.b).add(tri.c);
    });
    this.meanNormal.copy(normal).normalize();
    this.center.divideScalar(triangles.length * 3);
  }
}

// 无人机喷涂曲面的航线规划方案
export default class SurfaceRoutePlan {

  // 把三角面片根据法向 分配到不同的 平面区域
  static assignTrianglesToAreas(triangles) {
    const areas = [];
    const trianglesLeft = triangles.slice();
    while (trianglesLeft.length) {
      const firstTri = trianglesLeft.find(it => it.getArea() > ConfigRoute.ignoreArea);
      if (!firstTri) {
        return areas;
      }
      const inArea = [firstTri];
      const bfs = [firstTri];
      const firstNormal = firstTri.getNormal(new THREE.Vector3()).applyQuaternion(_quat);
      const firstSpherical = new THREE.Spherical().setFromVector3(firstNormal);
      const isHorizontal = ConfigRoute.isHorizontal(firstSpherical.phi);
      while (bfs.length) {
        const tri = bfs.shift();
        const neighbors = findTriangleNeighbors(trianglesLeft, inArea, tri, false);
        neighbors.forEach(tri => {
          if (tri.getArea() <= ConfigRoute.ignoreArea) {
            inArea.push(tri);
            bfs.push(tri);
            return;
          }
          tri.getNormal(_normal).applyQuaternion(_quat);
          _spherical.setFromVector3(_normal);
          const horizon = ConfigRoute.isHorizontal(_spherical.phi);
          if (isHorizontal !== horizon) {
            return;
          }
          // const diff = Math.abs(angleDiff(firstSpherical.theta, _spherical.theta));
          // const diff = Math.acos(firstNormal.dot(_normal))
          // if (diff <= ConfigRoute.azimuthAmplitude) {
            inArea.push(tri);
            bfs.push(tri);
          // }
        });
      }
      const triSet = new TriangleSet(inArea, isHorizontal);
      areas.push(triSet);
      arrAsubB(trianglesLeft, inArea);
    }
    return areas;
  }
}
